
#include <BSphere>
#include "IGjkConvexCast.h"
#include "IGjkPairDetector.h"
#include "IDetectorResult.h"

IGjkConvexCast::IGjkConvexCast(BShape* convexA, BShape* convexB, IVoronoiSimplexSolver* simplexSolver)
{
	_simplexSolver = simplexSolver;
	_shapeA = convexA;
	_shapeB = convexB;
}

bool IGjkConvexCast::calcTimeOfImpact(const BMatrix& sourceA, const BMatrix& targetA, const BMatrix& sourceB, const BMatrix& targetB, CastResult& caseResult)
{
	_simplexSolver->reset();

	BVector linVelA = targetA.position() - sourceA.position();
	BVector linVelB = targetB.position() - sourceB.position();

	BMatrix transformA = sourceA;
	BMatrix transformB = sourceB;

	IDetectorResult result;
	IGjkPairDetector gjk(_shapeA, _shapeB, _simplexSolver);
	gjk.closestPoints(sourceA, sourceB, result);

	if (result.hasResult)
	{
		BReal dist = result.distance;
		BVector n = result.normalB;
		BVector c = result.pointB;
		BVector r = (linVelA - linVelB);
		BReal lambda = 0;
		BReal lastLambda = 0;
		int numIter = 0;
		while (dist > REAL_EPSILON)
		{
			if (numIter++ > 64)
				return false;

			BReal projectedLinearVelocity = r.dot(n);
			BReal dLambda = dist / (projectedLinearVelocity);

			lambda = lambda - dLambda;

			if (lambda > 1)
				return false;

			if (lambda < 0)
				return false;

			if (lambda <= lastLambda)
				return false;

			lastLambda = lambda;

			transformA.setPosition((1 - lambda) * sourceA.position() + lambda * targetA.position());
			transformB.setPosition((1 - lambda) * sourceB.position() + lambda * targetB.position());

			gjk.closestPoints(transformA, transformB, result);
			if (result.hasResult)
			{
				if (result.distance < 0)
				{
					caseResult.fraction = lastLambda;
					n = result.normalB;
					caseResult.normal = n;
					caseResult.point = result.pointB;
					return true;
				}
				c = result.pointB;
				n = result.normalB;
				dist = result.distance;
			}
			else
			{
				return false;
			}
		}

		if (n.dot(r) >= 0)
			return false;

		caseResult.fraction = lambda;
		caseResult.normal = n;
		caseResult.point = c;
		return true;
	}

	return false;
}
